from graph import Graph
from point import Point
import sys
import pygame
import numpy as np
from collections import deque
class Astar(object):
    def __init__(self, Graph, start:Point, target:Point):
        self.Graph = Graph
        self.start = start
        self.target = target

        self.open_set = []
        self.close_set = []

    def g(self, p):
        return np.abs(p.x - self.start.x) + np.abs(p.y - self.start.y)
    def h(self, p):
        return np.abs(p.x - self.target.x) + np.abs(p.y - self.target.y)
    def f(self, p):
        return self.g(p) + 1.5* self.h(p)
############# define these function to test ####################
    def show_g_mat(self):
        g_mat = np.ones((G.m, G.n)) * sys.maxsize

        for i in range(G.m):
            for j in range(G.n):
                p = Point(i,j)

                g_mat[p.x][p.y] = self.g(p)
        print(g_mat)
    def show_h_mat(self):
        h_mat = np.ones((G.m, G.n)) * sys.maxsize

        for i in range(G.m):
            for j in range(G.n):
                p = Point(i,j)

                h_mat[p.x][p.y] = self.h(p)
        print(h_mat)
    def show_f_mat(self):
        f_mat = np.ones((G.m, G.n)) * sys.maxsize

        for i in range(G.m):
            for j in range(G.n):
                p = Point(i,j)

                f_mat[p.x][p.y] = self.f(p)

        print(f_mat)


    def BFS(self):
        Q = deque()
        visited = [] # I want to set visited = set(), but return error message: unhashable type: Point, may be set cannot contain object
        Q.append(self.start)
        visited.append(self.start)
        #note: for obejct how to check two object is equal
        while len(Q)!=0:
            V = Q.pop()
            #print("current: ", V)
            neighbors = G.get_neighbors(V)
            
            for neighbor in neighbors:
                neighbor.parent = V # set parent parent
                #print(neighbor)
                if neighbor == self.target:
                    p = neighbor
                    print("sucess")
                    print(p)
                    print('-'*10)
                    path = []
                    while p is not None:
                        print(p.x, p.y)
                        path.append((p.x,p.y))
                        p = p.parent
                    
                    return path

                if neighbor not in visited:
                    visited.append(neighbor)
                    Q.append(neighbor)
        return False



G = Graph((1000,1000),100)
s = Point(0,0)
t = Point(0,3)
A = Astar(G, s, t)
print(s == t)

neighbors = G.get_neighbors(s)
for p in neighbors:
    print(p.x, p.y)
print("######### g_mat #########")
A.show_g_mat()
print("#"*5, "h_mat", '#'*5)
A.show_h_mat()
print('#'*5, "f_mat", '#'*5)
A.show_f_mat()
A.BFS()

